#include "main.h"
#include "AutoControl.h"
#include "IRCtrol.h"
#include "LCD1602.h"
#include "interface.h"
#include "motor.h"
#include "speedCtrol.h"
#include <stdint.h>

// unsigned int speed_count = 0; // 占空比计数器 50次一周期
// char front_left_speed_duty = 0;
// char front_right_speed_duty = 0;
// char behind_left_speed_duty = 0;
// char behind_right_speed_duty = 0;

#ifndef CUSTOM_PWM_ENABLED
unsigned char tick_5ms = 0;   // 5ms计数器，作为主函数的基本周期
unsigned char tick_1ms = 0;   // 1ms计数器，作为电机的基本计数器
unsigned char tick_200ms = 0; // 刷新显示
#endif

char ctrl_comm = COMM_STOP;      // 控制指令
char ctrl_comm_last = COMM_STOP; // 上一次的指令

PID DirCtrl,   // Direction control PID, dependency for AutoControl/It Handler
    SpeedCtrl; // Speed control PID, dependency for AutoControl/It Handler

// IR Data Receive Flag.
// 1 New data available.
// 0 Not available.
// pointer usage causes variable type to be non-volatile.
unsigned char ir_rec_flag = 0;
unsigned char irDetected = 0;
// unsigned char bt_rec_flag = 0;   // BT Data Receive Flag
volatile uint32_t powerTime = 0; // Power-up time in milliseconds
volatile CommandPriority cmdPriority = cmd_Hyper;
char TIM_Enabled = 0;
int continue_time, // 控制指令持续时间
    control_time;  // 遥控态退出时限

static const int displayUpdatePeriod = 100, measurePeriod = 5;
static char displayTimeIsUp = 0, measureTimeIsUp = 0;
static uint32_t displayRecordTime = 0, measureRecordTime = 0, currTime = 0;

void speedUpdate(void) {
  currTime = powerTime;
  if (measureTimeIsUp) {
    MeasureSpeed();
    measureRecordTime = currTime;
    measureTimeIsUp = 0;
  } else if (currTime - measureRecordTime >= measurePeriod) {
    measureTimeIsUp = 1;
  }
  if (displayTimeIsUp) {
    LEDToggle(LED_PIN);
    LCD1602WriteSpeed(front_left_speed, front_right_speed);
    displayRecordTime = powerTime;
    displayTimeIsUp = 0;
  } else if (currTime - displayRecordTime >= displayUpdatePeriod) {
    displayTimeIsUp = 1;
  }
}

// const int statusExpireTime = 2000, commandExpireTime = 200;
// static uint32_t IRRecordTime = 0, ctrlRecordTime = 0;
static uint32_t cmdRecordTime = 0;
static char caseAltered = 0;
void IRTick(void) {
  currTime = powerTime;
  if (currTime - cmdRecordTime >= 5) {
    continue_time = continue_time > 0 ? continue_time - 1 : 0;
    control_time = control_time > 0 ? control_time - 1 : 0;
    cmdRecordTime = currTime;
  }
}
void IRMessageHandler() {
  if (ir_rec_flag) {
    auto2Manual();
    // IRRecordTime = currTime;
    // timeIsUp = 0;
    cmdPriority = min(cmd_IR, cmdPriority);
    ir_rec_flag = 0;
  }
  if (cmdPriority == cmd_IR) {
    if (control_time) {
      if (continue_time) {
        if (1 || (ctrl_comm != ctrl_comm_last)) {
          switch (ctrl_comm) {
          case COMM_STOP:
            assignSpeed(0);
            assignRotateSpeed(0);
            break;
          case COMM_UP:
            assignSpeed(60);
            assignRotateSpeed(0);
            break;
          case COMM_DOWN:
            assignSpeed(-60);
            assignRotateSpeed(0);
            break;
          case COMM_LEFT:
            assignSpeed(0);
            assignRotateSpeed(100);
            break;
          case COMM_RIGHT:
            assignSpeed(0);
            assignRotateSpeed(-100);
            break;
          default:
            assignSpeed(-60);
            assignRotateSpeed(0);
            break;
          }
        }
      } else {
        // stop car
        assignSpeed(0);
        assignRotateSpeed(0);
        ctrl_comm_last = ctrl_comm;
        ctrl_comm = COMM_STOP;
      }
      LCD1602WriteCommand(ctrl_comm);
    } else {
      cmdPriority = cmd_Default;
      manual2Auto();
    }
  }
}

const int testPeriod = 5000;
const int modeNum = 5;
static char testTimeIsUp = 0;
uint32_t testRecordTime = 0;
static int thisMode = 0;
void IRTest(void) {

  currTime = powerTime;
  if (testTimeIsUp) {
    continue_time = 40;
    control_time = 800;
    switch (thisMode) {
    case 0:
      ctrl_comm = COMM_STOP;
      break;
    case 1:
      ctrl_comm = COMM_UP;
      break;
    case 2:
      ctrl_comm = COMM_DOWN;
      break;
    case 3:
      ctrl_comm = COMM_LEFT;
      break;
    case 4:
      ctrl_comm = COMM_RIGHT;
      break;
    default:
      break;
    }
    thisMode = (thisMode + 1) % modeNum;
    ir_rec_flag = 1;

    testRecordTime = currTime;
    testTimeIsUp = 0;
  } else if (currTime - testRecordTime >= testPeriod) {
    testTimeIsUp = 1;
  }
}

int main() {
  TIM_Enabled = 0;
  delay_init();
  GPIOCLKInit(); // 调用IO口使能时钟函数
  UserLEDInit(); // 调用LED灯函数
  LCD1602Init();
  // FMInit();
  TIM2_Init(); // 已从10k提频至50k
  MotorInit();
  // ServoInit();
  IRCtrolInit();
  RedRayInit();
  MeasureInit();
  autoControlInit();
  // Delayms(500);
  cmdPriority = cmd_Default;
  TIM_Enabled = 1;
  while (1) {
    // LCD1602WriteCommand(ctrl_comm);
    // SearchRun();
    speedUpdate();
    // if (irDetected) {
    //   IRIntIsr();
    //   irDetected = 0;
    // }
    IRTick();
    IRMessageHandler();
    autoControl();

    // IRTest();
  }
}

void Error_Handler(void) {}
/**
 * @brief user implementation of assert_failed function used to handle
 * assertion failures.
 *
 * @param file
 * @param line
 */
void assert_failed(uint8_t *file, uint32_t line) {}